#include <string.h>
#include "driver/i2c.h"
#include "tea5767.h"
#include "event.h"
#include "key.h"

static xQueueHandle fm_evt_queue = NULL;
uint32_t freq_table[SAVE_FREQ_NUM] = {0};
uint16_t fm_num = 0;
uint32_t current_freq;
uint16_t freq_table_index;

static esp_err_t i2c_master_write_slave(i2c_port_t i2c_num, const uint8_t *data_wr, size_t size)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (TEA5767_ADDR << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
    i2c_master_write(cmd, data_wr, size, ACK_CHECK_EN);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

static esp_err_t i2c_master_read_slave(i2c_port_t i2c_num, uint8_t *data_rd, size_t size)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (TEA5767_ADDR << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
    i2c_master_read(cmd, data_rd, size, I2C_MASTER_ACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

unsigned long tea5767_Khz_to_pll(uint32_t Khz)
{
	return 4 * (Khz * 1000 + 225000) / TEA5767_CLK;
}

uint32_t tea5767_pll_to_Khz(unsigned long pll)
{
	return (pll* TEA5767_CLK / 4 - 225000) / 1000;
}

void tea5767_standby_mode(void)
{
	uint8_t data[5];
	data[0] = 0;
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;
	data[4] = 0;
	i2c_master_write_slave(TEA5767_I2C_POER_NUM, data, 5);
}

void tea5767_auto_search(void)
{
	struct event_struct fm_event;
	unsigned long pll;
	uint32_t khz = TEA5767_MIN_FREQ, freq;
	uint8_t data[5];
	uint8_t if_count;
	int i = 0;
	memset(freq_table, 0, SAVE_FREQ_NUM * sizeof(uint32_t));
	fm_event.event_type = (FM_EVENTS << 8) | FM_SEARCH_START;
	xQueueSend(evt_queue, &fm_event, portMAX_DELAY);
	while (khz < TEA5767_MAX_FREQ) {
		pll = tea5767_Khz_to_pll(khz);
		khz += 100;
		data[0] = ((pll>>8) & 0x3f) | TEA5767_SEARCH_MODE;
		data[1] = pll & 0xff;
		data[2] = TEA5767_SEARCH_UP | TEA5767_HLSI_EN | TEA5767_SSL_MODE_2;
		data[3] = TEA5767_XTAL_32768;
		data[4] = TEA5767_DTC_7P5_US;
		i2c_master_write_slave(TEA5767_I2C_POER_NUM, data, 5);
		vTaskDelay(100 / portTICK_RATE_MS);

		do {
			i2c_master_read_slave(TEA5767_I2C_POER_NUM, data, 5);
			vTaskDelay(100 / portTICK_RATE_MS);
		} while(((data[0] & (1 << 7)) == 0) && ((data[0] & (1 << 6)) == 0));

		if (data[0] & (1 << 6)) {
			printf("Band limit has been reached!\n");
			break;
		}
		if_count = data[2] & 0x7f;
		if ((if_count > IF_COUNT_MIN) && (if_count < IF_COUNT_MAX)) {
			if ((data[3] >> 4) > 7) {
				pll = ((data[0] & 0x3f) << 8) | data[1];
				freq = tea5767_pll_to_Khz(pll);
				for (i = 0; i < fm_num; i++) {
					if ((freq_table[i] / 10) == (freq / 10))
					//if (freq < freq_table[i] + 300)
						break;
				}

				/* find same freq */
				if (i != fm_num)
					continue;

				printf("save khz : %u\n", freq);
				freq_table[fm_num++] = freq;
				fm_event.event_type = (FM_EVENTS << 8) | FM_SEARCHING;
				fm_event.fm.freq = freq;
				xQueueSend(evt_queue, &fm_event, portMAX_DELAY);

				if (fm_num == SAVE_FREQ_NUM) {
					fm_num = SAVE_FREQ_NUM - 1;
					printf("out range of freq table!\n");
				}
			}
		}
	}
	printf("search finsh! find %d freq!\n", fm_num);
	fm_event.event_type = (FM_EVENTS << 8) | FM_SEARCH_FINISH;
	xQueueSend(evt_queue, &fm_event, portMAX_DELAY);
	printf("fm play!\n");
	fm_event.event_type = (FM_EVENTS << 8) | FM_PLAY;
	fm_event.fm.freq = freq_table[0];
	xQueueSend(evt_queue, &fm_event, portMAX_DELAY);
	freq_table_index = 0;
	current_freq = freq_table[0];
}

void tea5767_set_channel(uint32_t Khz)
{
	unsigned long pll;
	uint8_t data[5];
	struct event_struct fm_event;

	printf("%s freq %u\n", __func__, Khz);

	pll = tea5767_Khz_to_pll(Khz);
	data[0] = ((pll>>8) & 0x3f);
	data[1] = pll & 0xff;
	data[2] = TEA5767_HLSI_EN;
	data[3] = TEA5767_XTAL_32768;
	data[4] = TEA5767_DTC_7P5_US;
	i2c_master_write_slave(TEA5767_I2C_POER_NUM, data, 5);
	fm_event.event_type = (FM_EVENTS << 8) | FM_PLAY;
	fm_event.fm.freq = Khz;
	xQueueSend(evt_queue, &fm_event, portMAX_DELAY);
}

void fm_action(struct event_struct * evt)
{
	struct fm_evt_info fm_ei = {0};
	uint8_t long_press = evt->key_s.key_press;
	uint32_t gpio_num = evt->key_s.key_num;

	if (evt->event_type & (KEY_EVENTS << 8)) {
		if (gpio_num == KEY_LEFT_GPIO) {
			if (long_press == 0) {
				current_freq -= 10;
				if (freq_table_index > 0) {
					if (current_freq < freq_table[freq_table_index - 1]) {
						freq_table_index--;
						current_freq = freq_table[freq_table_index];
					}
				}
			} else {
				if (freq_table_index > 0) {
					freq_table_index--;
					current_freq = freq_table[freq_table_index];
				}
			}
		} else if (gpio_num == KEY_RIGHT_GPIO) {
			if (long_press == 0) {
				current_freq += 10;
				printf("1 %u, index = %d\n", current_freq, freq_table_index);
				if (freq_table_index < fm_num - 1) {
					if (current_freq > freq_table[freq_table_index + 1]) {
						freq_table_index++;
						current_freq = freq_table[freq_table_index];
						printf("2 %u, index = %d\n", current_freq, freq_table_index);
					}
				}
			} else {
				if (freq_table_index < fm_num - 1) {
					freq_table_index++;
					current_freq = freq_table[freq_table_index];
				}
			}
		} else {
			return;
		}

		fm_ei.event = FM_EVT_FREQ;
		fm_ei.value = current_freq;
		printf("3 %u\n", fm_ei.value);
		xQueueSend(fm_evt_queue, &fm_ei, portMAX_DELAY);
	}
}
void fm_start(uint8_t enable)
{
	static uint8_t fm_on = 0;
	struct fm_evt_info fm_ei = {0};
	printf("%s enable = %d\n", __func__, enable);
	if (enable) {
		if (fm_on == 0) {
			fm_ei.event = FM_EVT_SEARCH;
			fm_ei.value = 0;
			xQueueSend(fm_evt_queue, &fm_ei, portMAX_DELAY);
		}
		//if (fm_num) {
		//	fm_ei.event = FM_EVT_FREQ;
		//	fm_ei.value = freq_table[0];
		//	xQueueSend(fm_evt_queue, &fm_ei, portMAX_DELAY);
		//}
	} else {
		tea5767_standby_mode();
	}

	fm_on = enable;
}

static void tea5767_handle(void* arg)
{
	struct fm_evt_info fm_ei = {0};
	tea5767_standby_mode();
	while(1) {
		if(xQueueReceive(fm_evt_queue, &fm_ei, portMAX_DELAY)) {
			switch (fm_ei.event) {
				case FM_EVT_SEARCH:
					tea5767_auto_search();
				break;
				case FM_EVT_FREQ:
				printf("4 %u\n", fm_ei.value);
					tea5767_set_channel(fm_ei.value);
				break;
				default:
				break;
			}
		}
	}
}

esp_err_t tea5767_init(void)
{
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = TEA5767_I2C_SDA_IO,         // select GPIO specific to your project
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_io_num = TEA5767_I2C_SCL_IO,         // select GPIO specific to your project
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = TEA5767_I2C_FREQ,  // select frequency specific to your project
        // .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */
    };
    if(i2c_param_config(TEA5767_I2C_POER_NUM, &conf) != ESP_OK) {
        return 1;
	}

    if(i2c_driver_install(TEA5767_I2C_POER_NUM, I2C_MODE_MASTER, 0, 0, 0) != ESP_OK) {
        return 2;
	}

	fm_evt_queue = xQueueCreate(5, sizeof(struct fm_evt_info));
	xTaskCreate(tea5767_handle, "tea5767_handle_func", 2048, NULL, 8, NULL);

    return ESP_OK;
}
